#ifndef _WF_TASK1_TALKER_HPP_
#define _WF_TASK1_TALKER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <cv_bridge/cv_bridge.h>
#include <string>
#include <iostream>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include <opencv2/highgui.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl_conversions/pcl_conversions.h>


namespace wf
{
    namespace talker
    {
        using TransformStamped = geometry_msgs::msg::TransformStamped;
        using Image = sensor_msgs::msg::Image;
        using PointCloud2 = sensor_msgs::msg::PointCloud2;
        using PoseStamped = geometry_msgs::msg::PoseStamped;

        class TalkerNode : public rclcpp::Node
        {
            private:
                std::string image_path_;
                std::string pcd_path_;
                std::string source_frame_;
                std::string target_frame_;
                std::vector<double> transform_;
                std::vector<double> pose_;

                TransformStamped transform_stamped_;
                Image image_;
                PointCloud2 pcd2_;
                PoseStamped pose_stamped_;

                std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
                rclcpp::Publisher<Image>::SharedPtr image_publisher_;
                rclcpp::Publisher<PointCloud2>::SharedPtr pcd2_publisher_;
                rclcpp::Publisher<PoseStamped>::SharedPtr pose_publisher_;

                void init();
                TransformStamped load_tf(const std::vector<double> &transform, const std::string frame_id, const std::string child_frame_id);
                Image load_image(const std::string path, const std::string frame_id);
                PointCloud2 load_pointcloud2(const std::string path, const std::string frame_id);
                PoseStamped load_pose(const std::vector<double> &pose, const std::string frame_id);

                void broadcast_static_tf(std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster, TransformStamped &tf);

                template <typename T>
                void publish(typename rclcpp::Publisher<T>::SharedPtr publisher, T &message);

                void publish_all(const double rate);

            public:
                explicit TalkerNode(const rclcpp::NodeOptions &options);
        };
        
    } // namespace talker
    
} // namespace wf


#endif